#include <iostream>
#include "rungeKuttaMethod.h"

RungeKuttaMethod::RungeKuttaMethod() : IntegrationMethod(){
}

RungeKuttaMethod::RungeKuttaMethod(double _deltaT, double _g, std::vector<State> _s0,
                                   std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                                   bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                                   : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
}

RungeKuttaMethod::~RungeKuttaMethod(){
}


void RungeKuttaMethod::calculate(bool, unsigned int _fabricPosition, double _fraction){
    //guardando dados
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();
    State k1, k2, k3, k4, aux;

    currentDeltaT = deltaT / _fraction;

    //calculando
    k1 = f(currentState.at(_fabricPosition), _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT / 2.0)));
    aux.update(currentState.at(_fabricPosition), currentDeltaT / 2.0);
    k2 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k2 * (currentDeltaT / 2.0)));
    aux.update(currentState.at(_fabricPosition), currentDeltaT / 2.0);
    k3 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k3 * currentDeltaT));
    aux.update(currentState.at(_fabricPosition), currentDeltaT);
    k4 = f(aux, _fabricPosition);

    s = currentState.at(_fabricPosition) + ((currentDeltaT / 6.0) * (k1 + (2.0 * k2) + (2.0 * k3) + k4 ));




    //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) = s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);

}
